#!/usr/bin/env python3
# -*- coding: utf-8 -*- 

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from _ModeConfig import ModeConfig
import sys
from lib.RobotBase import RobotBase
from RobotCar.CarRobot import CarRobot


global robotBase
global car


def scanIds():
    idPs=robotBase.ScanIdsWithPostions()
    fIds = robotBase.QueryIds(4,idPs[0],idPs[1])
    bIds = robotBase.QueryIds(11,idPs[0],idPs[1])
    l1Ids = robotBase.QueryIds(8,idPs[0],idPs[1])
    l2Ids = robotBase.QueryIds(8,idPs[0],idPs[1])
    l3Ids = robotBase.QueryIds(10,idPs[0],idPs[1])
    l4Ids = robotBase.QueryIds(11,idPs[0],idPs[1])
    
    r1Ids = robotBase.QueryIds(4,idPs[0],idPs[1])
    r2Ids = robotBase.QueryIds(7,idPs[0],idPs[1])
    r3Ids = robotBase.QueryIds(6,idPs[0],idPs[1])
    r4Ids = robotBase.QueryIds(6,idPs[0],idPs[1])    
    sortIds = [fIds[0],bIds[0],l1Ids[0],l2Ids[0],l3Ids[0],l4Ids[0],r1Ids[0],r2Ids[0],r3Ids[0],r4Ids[0]]
    return sortIds



def modeConfig_cb(data):
    rospy.loginfo(rospy.get_caller_id() + "Recevied modeConfig %s", str(data.mode))
    sortIds=[data.f_id,data.b_id,data.l1_id,data.l2_id,data.l3_id,data.l4_id,data.r1_id,data.r2_id,data.r3_id,data.r4_id]

    isZero=data.f_id==0 and data.b_id==0 and data.l1_id==0 and data.l2_id==0 and data.l3_id==0 and data.l4_id==0 and data.r1_id==0 and data.r2_id==0 and  data.r3_id==0 and data.r4_id==0 
    if(isZero):
       sortIds=scanIds()
    car.BindIds(sortIds,data.mode)

def cmd_cb(data):
    x=data.linear.x
    y=data.linear.y

    rotatez=data.angular.z
    
    if(x!=0):
        car.SetRunningSpeedWithMS(x)
        car.GoForward()

    if(y!=0):
        car.SetRunningSpeedWithMS(y)
        car.RunLeft()
            
          
    if(rotatez!=0):
        car.SetRunningSpeedWithRadS(rotatez)
        if(rotatez>0):
            car.TurnLeft()
            car.lib.SetLEDBel(2)
        if(rotatez<0):
            car.TurnRight()  
            car.lib.SetLEDBel(1)

    if(x==0 and y==0 and rotatez==0):
        car.Stop() 
        car.lib.SetLEDBel(5)  
    
    rospy.loginfo("Recevied Twist x,y,rotate %s", str(x)+","+str(y)+","+str(rotatez))


def listener():
    rospy.init_node('minibot_agv_driver', anonymous=True)
    rospy.Subscriber("car_config", ModeConfig, modeConfig_cb)
    rospy.Subscriber("cmd_vel", Twist, cmd_cb)
    rospy.spin()




if __name__ == '__main__':

    robotBase = RobotBase()
    car = CarRobot(robotBase)
    robotBase.ConnectRobot('/dev/minibot',8899)
    #car.BindIds([0,0,21,0,0,22,23,0,0,24],6)
    sortIds=scanIds()
    car.BindIds(sortIds,6)
    listener()
